// system
#include <string.h>
#include <stdlib.h>
#include <stdio.h>
// cJSON
#include "cJSON.h"
// gtrack
#include "gtrack_int.h"
#include "gtrack_config.h"
#include "typedef.h"


cJSON* gtrack_read_json_file(const char* filename)
{
	FILE* f = fopen(filename, "rb");
	if (!f)
	{
		return NULL;
	}

	fseek(f, 0, SEEK_END);
	long len = ftell(f);
	fseek(f, 0, SEEK_SET);

	char* data = (char*)gtrack_alloc(1, len + 1);
	if (fread(data, 1, len, f) != len)
	{
		fclose(f);
		return NULL;
	}

	fclose(f);
	data[len] = '\0';

	cJSON* root = cJSON_Parse(data);
	gtrack_free(data, len + 1);
	return root;
}

int gtrack_write_json_file(const char* filename, const cJSON* root)
{
	// 生成字符串并保存
	char* json_str = cJSON_Print(root);
	FILE* f = fopen(filename, "w");
	int res = 0;
	if (f)
	{
		fputs(json_str, f);
		fclose(f);
		res = 1;
	}
	free(json_str);
	return res;
}

void gtrack_config_save(const char* filename, const void* config)
{
	const GTRACK_moduleConfig* p_config = (const GTRACK_moduleConfig*)config;
	cJSON* root = cJSON_CreateObject();

	/**  @brief  添加基础参数配置 */
	cJSON_AddNumberToObject(root, "max_point_num", p_config->maxNumPoints);
	cJSON_AddNumberToObject(root, "max_track_num", p_config->maxNumTracks);
	cJSON_AddNumberToObject(root, "init_radial_velocity", p_config->initialRadialVelocity);
	cJSON* max_acc = cJSON_CreateFloatArray(p_config->maxAcceleration, 3);
	cJSON_AddItemToObject(root, "max_acceleration", max_acc);

	/**  @brief  添加雷达参数配置 */
	cJSON* radar_obj = cJSON_CreateObject();
	const GTRACK_RadarParams* radar_conf = &(p_config->radarParams);
	cJSON_AddNumberToObject(radar_obj, "max_velocity_limit", radar_conf->maxRadialVelocity);
	cJSON_AddNumberToObject(radar_obj, "velocity_resolution", radar_conf->radialVelocityResolution);
	cJSON_AddNumberToObject(radar_obj, "range_resolution", radar_conf->rangeResolution);
	cJSON_AddNumberToObject(radar_obj, "rad_resolution", radar_conf->radResolution);
	cJSON_AddNumberToObject(radar_obj, "frame_rate", radar_conf->deltaT);
	cJSON_AddItemToObject(root, "radar_param", radar_obj);

	/**  @brief  高级参数 */
	const GTRACK_advancedParameters* adv_conf = p_config->advParams;
	if (adv_conf != NULL)
	{
		cJSON* adv_obj = cJSON_CreateObject();
		/**  @brief  添加场景参数配置 */
		const GTRACK_sceneryParams* scen_conf = adv_conf->sceneryParams;
		cJSON* scen_obj = cJSON_CreateObject();
		cJSON* bboxs = cJSON_CreateArray();
		for (int i = 0; i < scen_conf->numBoundaryBoxes; i++)
		{
			cJSON* bbox = cJSON_CreateFloatArray(scen_conf->boundaryBox[i].box, 6);
			cJSON_AddItemToArray(bboxs,  bbox);
		}
		cJSON_AddItemToObject(scen_obj, "boundary_boxs", bboxs);
		cJSON* sboxs = cJSON_CreateArray();
		for (int i = 0; i < scen_conf->numStaticBoxes; i++)
		{	
			cJSON* sbox = cJSON_CreateFloatArray(scen_conf->staticBox[i].box, 6);
			cJSON_AddItemToArray(sboxs, sbox);
		}
		cJSON_AddItemToObject(scen_obj, "static_boxs", sboxs);
		cJSON_AddItemToObject(adv_obj, "scenery_param", scen_obj);

		/**  @brief  添加门限参数配置 */
		const GTRACK_gatingParams* gating_conf = adv_conf->gatingParams;
		cJSON* gating_obj = cJSON_CreateObject();
		cJSON_AddNumberToObject(gating_obj, "gain", gating_conf->gain);
		cJSON* gate_limits = cJSON_CreateFloatArray(gating_conf->limitsArray, 4);
		cJSON_AddItemToObject(gating_obj, "gate_limits", gate_limits);
		cJSON_AddItemToObject(adv_obj, "gating_param", gating_obj);

		/**  @brief  添加分配参数配置 */
		const GTRACK_allocationParams* alloc_conf = adv_conf->allocationParams;
		cJSON* alloc_obj = cJSON_CreateObject();
		cJSON_AddNumberToObject(alloc_obj, "max_distance_thd", alloc_conf->maxDistanceThre);
		cJSON_AddNumberToObject(alloc_obj, "max_vel_thd", alloc_conf->maxVelThre);
		cJSON_AddNumberToObject(alloc_obj, "points_thd", alloc_conf->pointsThre);
		cJSON_AddNumberToObject(alloc_obj, "snr_thd", alloc_conf->snrThre);
		cJSON_AddNumberToObject(alloc_obj, "velocity_thd", alloc_conf->velocityThre);
		cJSON_AddItemToObject(adv_obj, "allocation_param", alloc_obj);

		/**  @brief  添加展开参数配置，暂不提供接口，可读不写 */
		// unrolling_param ... 

		/**  @brief  添加合并参数配置，暂不提供接口，可读不写 */
		// merge_param ...

		cJSON_AddItemToObject(root, "advanced_param", adv_obj);
	}
	gtrack_write_json_file(filename, root);
	cJSON_Delete(root);
}

void get_default_config(GTRACK_moduleConfig* p_config)
{
	/**  @brief  基本参数 */
	p_config->initialRadialVelocity = 0.f;
	p_config->maxNumPoints = 1024;
	p_config->maxNumTracks = 250;

#ifdef GTRACK_3D
	p_config->stateVectorType = GTRACK_STATE_VECTORS_3DA;
#else
	p_config->stateVectorType = GTRACK_STATE_VECTORS_2DA;
#endif

	p_config->maxAcceleration[0] = 0.5f;
	p_config->maxAcceleration[1] = 2.f;
	p_config->maxAcceleration[2] = 0.f;

	/**  @brief  雷达参数 */
	{
		// TDM
		//    config.radarParams.deltaT = 0.15f;
		//    config.radarParams.maxRadialVelocity = 11 * 6.1579f;
		//    config.radarParams.radialVelocityResolution = 0.0962f;
		//    config.radarParams.rangeResolution = 1.491f;
		//    config.radarParams.radResolution = (2 / 80.f) * 2;
		// FIX_CP512_REMOTE
		//    config.radarParams.deltaT = 0.15f;
		//    config.radarParams.maxRadialVelocity = 60.f;
		//    config.radarParams.radialVelocityResolution = 0.0756f;
		//    config.radarParams.rangeResolution = 1.5904f;
		//    config.radarParams.radResolution = (2 / 80.f) * 2;
		// FIX_CP512_NEAR
		//    config.radarParams.deltaT = 0.15f;
		//    config.radarParams.maxRadialVelocity = 6 * 9.66f;
		//    config.radarParams.radialVelocityResolution = 0.0755f;
		//    config.radarParams.rangeResolution = 0.5859f;
		//    config.radarParams.radResolution = (2 / 32.f) * 0.5f;

		GTRACK_RadarParams radar_conf = { 60.f, 0.5f, 1.5f, 0.04f, 0.15f };
		p_config->radarParams = radar_conf;
	}

	if (p_config->advParams != NULL)
	{
		/**  @brief  门限参数 */
		if(p_config->advParams->gatingParams != NULL)
		{
			GTRACK_gatingParams gating_conf = { 16.f, { 8.0f, 4.0f, 2.f, 3.f } };
			*(p_config->advParams->gatingParams) = gating_conf;
		}
		/**  @brief  分配参数 */
		if (p_config->advParams->allocationParams != NULL)
		{
			GTRACK_allocationParams alloc_conf = { 5.f,	1.0f,	2U, 3.0f, 1.0f };
			*(p_config->advParams->allocationParams) = alloc_conf;
		}
		/**  @brief  展开参数 */
		if (p_config->advParams->unrollingParams != NULL)
		{
			GTRACK_unrollingParams unroll_conf = { 0.5f, 0.1f };
			*(p_config->advParams->unrollingParams) = unroll_conf;
		}
		/**  @brief  合并参数 */
		if (p_config->advParams->mergeParams != NULL)
		{
			GTRACK_mergeParams merge_conf = { 10U, 2.0f, 1.0f };
			*(p_config->advParams->mergeParams) = merge_conf;
		}
		/**  @brief  场景参数 */
		if (p_config->advParams->sceneryParams != NULL)
		{
			GTRACK_sceneryParams scen_conf = { 0, { { 0.f, 0.f, 0.f, 0.f }, { 0.f, 0.f, 0.f, 0.f }, { 0.f, 0.f, 0.f, 0.f } }, 0, { { 0.f, 0.f, 0.f, 0.f }, { 0.f, 0.f, 0.f, 0.f }, { 0.f, 0.f, 0.f, 0.f } } };
			*(p_config->advParams->sceneryParams) = scen_conf;
		}
	}
}

void parse_json_data(cJSON* js, GTRACK_moduleConfig* p_config)
{
	if (!js)
	{
		return;
	}

	/**  @brief  基础参数 */
	cJSON* max_point_num = cJSON_GetObjectItemCaseSensitive(js, "max_point_num");
	cJSON* max_track_num = cJSON_GetObjectItemCaseSensitive(js, "max_track_num");
	cJSON* init_radial_velocity = cJSON_GetObjectItemCaseSensitive(js, "init_radial_velocity");
	cJSON* max_acceleration = cJSON_GetObjectItemCaseSensitive(js, "max_acceleration");
	cJSON* radar_param = cJSON_GetObjectItemCaseSensitive(js, "radar_param");
	cJSON* advanced_param = cJSON_GetObjectItemCaseSensitive(js, "advanced_param");
	parse_u_short_data(max_point_num, &(p_config->maxNumPoints));
	parse_u_short_data(max_track_num, &(p_config->maxNumTracks));
	parse_float_data(init_radial_velocity, &(p_config->initialRadialVelocity));
	parse_float_array(max_acceleration, p_config->maxAcceleration, 3);

	/**  @brief  雷达参数 */
	if (cJSON_IsObject(radar_param))
	{
		GTRACK_RadarParams* radar_conf = &(p_config->radarParams);
		cJSON* max_velocity_limit = cJSON_GetObjectItemCaseSensitive(radar_param, "max_velocity_limit");
		cJSON* velocity_resolution = cJSON_GetObjectItemCaseSensitive(radar_param, "velocity_resolution");
		cJSON* range_resolution = cJSON_GetObjectItemCaseSensitive(radar_param, "range_resolution");
		cJSON* rad_resolution = cJSON_GetObjectItemCaseSensitive(radar_param, "rad_resolution");
		cJSON* frame_rate = cJSON_GetObjectItemCaseSensitive(radar_param, "frame_rate");
		parse_float_data(max_velocity_limit, &(radar_conf->maxRadialVelocity));
		parse_float_data(velocity_resolution, &(radar_conf->radialVelocityResolution));
		parse_float_data(range_resolution, &(radar_conf->rangeResolution));
		parse_float_data(rad_resolution, &(radar_conf->radResolution));
		parse_float_data(frame_rate, &(radar_conf->deltaT));
	}

	/**  @brief  高级参数 */
	if (cJSON_IsObject(advanced_param))
	{
		cJSON* scenery_param = cJSON_GetObjectItemCaseSensitive(advanced_param, "scenery_param");
		cJSON* gating_param = cJSON_GetObjectItemCaseSensitive(advanced_param, "gating_param");
		cJSON* allocation_param = cJSON_GetObjectItemCaseSensitive(advanced_param, "allocation_param");
		cJSON* unrolling_param = cJSON_GetObjectItemCaseSensitive(advanced_param, "unrolling_param");
		cJSON* merge_param = cJSON_GetObjectItemCaseSensitive(advanced_param, "merge_param");
		if (cJSON_IsObject(scenery_param))
		{
			GTRACK_sceneryParams* scen_conf = p_config->advParams->sceneryParams;
			cJSON* boundary_boxs = cJSON_GetObjectItemCaseSensitive(scenery_param, "boundary_boxs");
			cJSON* static_boxs = cJSON_GetObjectItemCaseSensitive(scenery_param, "static_boxs");
			if (cJSON_IsArray(boundary_boxs))
			{
				int boundary_boxs_size = cJSON_GetArraySize(boundary_boxs);
				boundary_boxs_size = MIN(boundary_boxs_size, GTRACK_MAX_BOUNDARY_BOXES);
				scen_conf->numBoundaryBoxes = boundary_boxs_size;
				cJSON* child = boundary_boxs->child;
				for (int i = 0; i < boundary_boxs_size; i++)
				{
					parse_float_array(child, scen_conf->boundaryBox[i].box, 6);
					child = child->next;
				}
			}
			if (cJSON_IsArray(static_boxs))
			{
				int static_boxs_size = cJSON_GetArraySize(static_boxs);
				static_boxs_size = MIN(static_boxs_size, GTRACK_MAX_STATIC_BOXES);
				scen_conf->numStaticBoxes = static_boxs_size;
				cJSON* child = static_boxs->child;
				for (int i = 0; i < static_boxs_size; i++)
				{
					parse_float_array(child, scen_conf->staticBox[i].box, 6);
					child = child->next;
				}
			}
		}
		if (cJSON_IsObject(gating_param))
		{
			GTRACK_gatingParams* gating_conf = p_config->advParams->gatingParams;
			cJSON* gain = cJSON_GetObjectItemCaseSensitive(gating_param, "gain");
			cJSON* gate_limits = cJSON_GetObjectItemCaseSensitive(gating_param, "gate_limits");
			parse_float_data(gain, &(gating_conf->gain));
			parse_float_array(gate_limits, gating_conf->limitsArray, 4);
		}
		if (cJSON_IsObject(allocation_param))
		{
			GTRACK_allocationParams* alloc_conf = p_config->advParams->allocationParams;
			cJSON* max_distance_thd = cJSON_GetObjectItemCaseSensitive(allocation_param, "max_distance_thd");
			cJSON* max_vel_thd = cJSON_GetObjectItemCaseSensitive(allocation_param, "max_vel_thd");
			cJSON* points_thd = cJSON_GetObjectItemCaseSensitive(allocation_param, "points_thd");
			cJSON* snr_thd = cJSON_GetObjectItemCaseSensitive(allocation_param, "snr_thd");
			cJSON* velocity_thd = cJSON_GetObjectItemCaseSensitive(allocation_param, "velocity_thd");
			parse_float_data(max_distance_thd, &(alloc_conf->maxDistanceThre));
			parse_float_data(max_vel_thd, &(alloc_conf->maxVelThre));
			parse_u_short_data(points_thd, &(alloc_conf->pointsThre));
			parse_float_data(snr_thd, &(alloc_conf->snrThre));
			parse_float_data(velocity_thd, &(alloc_conf->velocityThre));
		}
		if (cJSON_IsObject(unrolling_param))
		{
			GTRACK_unrollingParams* unrolling_conf = p_config->advParams->unrollingParams;
			cJSON* alpha = cJSON_GetObjectItemCaseSensitive(unrolling_param, "alpha");
			cJSON* confidence = cJSON_GetObjectItemCaseSensitive(unrolling_param, "confidence");
			parse_float_data(alpha, &(unrolling_conf->alpha));
			parse_float_data(confidence, &(unrolling_conf->confidence));
		}
		if (cJSON_IsObject(merge_param))
		{
			GTRACK_mergeParams* merge_conf = p_config->advParams->mergeParams;
			cJSON* merge_count_thd = cJSON_GetObjectItemCaseSensitive(merge_param, "merge_count_thd");
			cJSON* merge_dist_thd = cJSON_GetObjectItemCaseSensitive(merge_param, "merge_dist_thd");
			cJSON* merge_vel_thd = cJSON_GetObjectItemCaseSensitive(merge_param, "merge_vel_thd");
			parse_u_short_data(merge_count_thd, &(merge_conf->mergeCountThre));
			parse_float_data(merge_dist_thd, &(merge_conf->mergeDistThre));
			parse_float_data(merge_vel_thd, &(merge_conf->mergeSpeedThre));
		}
	}
}

int gtrack_config_load(const char* filename, void* config)
{
	GTRACK_moduleConfig* p_config = (GTRACK_moduleConfig*)config;
	get_default_config(p_config);
	cJSON* root = gtrack_read_json_file(filename);
	if (!root)
	{
		const char* error_ptr = cJSON_GetErrorPtr();
		printf("JSON 解析错误: %s\n", error_ptr);
		return 0;

	}
	parse_json_data(root, p_config);
	return 1;
}
